% load('data/test_kalman.mat'); 
% test_kalman(A,B,C,D, past_acc_inst-ones(20,1)*m_inst_count_vec, past_rt-m_rt_vec,x0)
function x_last = test_kalman(A,B,C,D, past_acc_inst,past_rt,x0,t_)
% A = [1.1269   -0.4940    0.1129
%      1.0000         0         0
%           0    1.0000         0];
% 
% B = [-0.3832
%       0.5919
%       0.5191];
% 
% C = [1 0 0]; 

Q = 1; R = 0;
 
%Generate a sinusoidal input and process and measurement noise vectors and
% t = [0:19]';
t=[0:t_-1]; 
% u = sin(t/5);
 %u=4*sin(ones(size(t,1),1)*rand(1,11)+(t/5)*ones(1,11))+8; 
u=past_acc_inst;


n = length(t)
randn('seed',0)
% w = sqrt(Q)*randn(n,1);
% v = sqrt(R)*randn(n,1);

% Use process noise w and measurement noise v generated above
sys = ss(A,B,C,0,-1);
% y = lsim(sys,[u w]);      % w = process noise
 y=past_rt; 
 yv = y; 
 %  yv = y + v;             % v = measurement noise

%you can implement the time-varying filter with the following for loop.

P = B*Q*B';         % Initial error covariance
x = x0; % zeros(3,1);     % Initial condition on the state
ye = zeros(length(t),1);
ycov = zeros(length(t),1); 
xAll=zeros(3,t_);
for i=1:length(t)
  % Measurement update
  Mn = P*C'/(C*P*C'+R);
  x = x + Mn*(yv(i)-C*x);   % x[n|n]
  P = (eye(3)-Mn*C)*P;      % P[n|n]
  xAll(:,i)=x;
  x_last = x; 
  
  ye(i) = C*x;
  errcov(i) = C*P*C';

  % Time update
  x = A*x + B(:,1:11)*[u(i,:)]';        % x[n+1|n]
  P = A*P*A' + B*Q*B';     % P[n+1|n]
end

kk=0; 
% %You can now compare the true and estimated output graphically.
% subplot(211), plot(t,y,'--',t,ye,'-')
% title('Time-varying Kalman filter response')
% xlabel('No. of samples'), ylabel('Output')
% subplot(212), plot(t,y-yv,'-.',t,y-ye,'-')
% xlabel('No. of samples'), ylabel('Output')